#include <armadillo>
#include "Intergrate.hpp"

using namespace arma;


double Fi(double x)
{
	return x/(1-pow(x,2));
}

double CauchyNormalE(double x,void *alpha)
{
	double g=((double *)alpha)[0];
	double s=((double *)alpha)[1];
	double m=((double *)alpha)[2];
	double res=(1/(PI*g*sqrt(2*PI)*s))*Fi(x)/(1+(pow(Fi(x),2))/pow(g,2))*exp(-(1/(2*pow(s,2)))*pow(Fi(x)-m,2))*((1+pow(x,2))/pow((1-pow(x,2)),2));
	return res;
}	
double CauchyNormalV(double x, void* alpha)
{
	double g=((double *)alpha)[0];
	double s=((double *)alpha)[1];
	double m=((double *)alpha)[2];
	double res=(1/(PI*g*sqrt(2*PI)*s))*pow(Fi(x),2)/(1+(pow(Fi(x),2))/pow(g,2))*exp(-(1/(2*pow(s,2)))*pow(Fi(x)-m,2))*((1+pow(x,2))/pow((1-pow(x,2)),2));

	return res;
}
double CauchyNormalZ(double x,void* alpha)
{
	double g=((double *)alpha)[0];
	double s=((double *)alpha)[1];
	double m=((double *)alpha)[2];
	double res=(1/(PI*g*sqrt(2*PI)*s))/(1+(pow(Fi(x),2))/pow(g,2))*exp(-(1/(2*pow(s,2)))*pow(Fi(x)-m,2))*((1+pow(x,2))/pow((1-pow(x,2)),2));
	return res;
}


class EPst
{
	public:

	EPst(void)
	{	
		boost::math::normal_distribution<> *d=new boost::math::normal_distribution<>(0,1); 
		_d=d;
		_I=new Integrate(0.00000001);
	}
	~EPst()
	{
		delete _d;
		delete _I;
	}

	void operator()(mat X,double *Yt,mat m,double lambda, int npass=4)
	{
		int p=X.n_cols;
		int n=X.n_rows;
		mat mu(p,1);
		mat Q(p,p);
		int kk=0;
		mat I(p,p);
		I.eye();
	       double *Y=new double[n];	
		for(int i=0;i<n;i++)
		{
			if(Yt[i]==0)
			{
				Y[i]=-1;
			}else{
				Y[i]=1;
			}
		}
		Q.eye();
		Q=(1/lambda)*Q;	
		cube Qi(n+p,p,p);
		Qi.fill(0);
		mat Qmui(n+p,p);
		Qmui.fill(0);
		mat z(n,1);
		mat ps(n,1);
		mat pmi(n,1);
		mat v(n,1);
		mat Sigma(p,p);
		for(int k=0;k<npass;k++)
		{
			for(int i=0;i<n+p;i++)
			{
				kk=0;
				mat oldmu=mu;
				mat oldQ=Q;
				mat temp=Qi(span(i,i),span::all,span::all);
				mat Qmi=Q-temp;
				//mat Lmi=chol(Qmi);
				mat mumi=inv(Qmi)*(Q*mu-Qmui(i,span::all).t());
				if(i<n){
					mat varz=1+X(i,span::all)*(inv(Qmi)*X(i,span::all).t());
					mat ratz=Y[i]*X(i,span::all)*mumi/sqrt(varz(0,0));
					//cout << "p" << << "\n";
					double pz=cdf(*_d,ratz(0,0));
					mat diffpz=(pdf(*_d,ratz(0,0))/sqrt(varz(0,0)))*Y[i]*X(i,span::all);
					double a=(1/sqrt(varz(0,0)))*(pdf(*_d,ratz(0,0))/pz);
					mat b=pow(a,2)+a*Y[i]*(X(i,span::all)*mumi)/varz(0,0);
					v(i,0)=1/b(0,0)-varz(0,0)+1;
					mat foo=Y[i]*X(i,span::all)*mumi+a/b(0,0);
					pmi(i,0)=foo(0,0);
					mat bar=pz*sqrt(1+(varz(0,0)-1)/v(i,0))*exp(pow(a,2)/(2*b));
					ps(i,0)=bar(0,0);
					mat foo1=inv(Qmi);
					mu=mumi+foo1*(diffpz.t()/pz);
					
					mat diff2pz=(diff_normpdf(ratz(0,0))/varz(0,0))*(X(i,span::all).t()*X(i,span::all));
					mat toto=inv(Qmi)*(diff2pz/pz)+I;
					Sigma=toto*inv(Qmi)-(mumi-mu)*((mumi-mu).t());
					Q=inv(Sigma);
					mat ncont=Q-Qmi;
					Qi(span(i,i),span::all,span::all)=ncont;
			//	cout << Q << "\n";	
					Qmui(i,span::all)=(Q*mu-Qmi*mumi).t();
				}else{
					mat ER=inv(Qmi);
					double* alphat=new double[3];
					alphat[1]=sqrt(ER(kk,kk));
					alphat[2]=mumi(kk,0);
					alphat[0]=sqrt(lambda);
						
				 	double ze=(*_I)(-1,1,&(CauchyNormalZ),alphat);	
				 	double me=(*_I)(-1,1,&(CauchyNormalE),alphat)/ze;	
				 	double ve=(*_I)(-1,1,&(CauchyNormalV),alphat)/ze-pow((me),2);	
					
					mu(kk,0)=me;	
					Sigma(kk,kk)=ve;
					Q=inv(Sigma);
					mat ncont=Q-Qmi;
					Qi(span(i,i),span::all,span::all)=ncont;
			//	cout << Q << "\n";	
					Qmui(i,span::all)=(Q*mu-Qmi*mumi).t();
					delete alphat;
					kk++;	
				}
					
			}
		
		}
		_m=mu;
		_S=inv(Q);
		//cout << _S;
		//std::cout << _m;
		delete[] Y;

	}
	double diff_normpdf(double x)
	{
		return -x*pdf(*_d,x);
	}
	mat Get_Sig(void){return _S;}
	mat Get_Mu(void){return _m;}
	private:
	mat _m;
	mat _S;
	Integrate *_I;
	boost::math::normal_distribution<> *_d; 
};

